#!/usr/bin/env python3

import math
from functools import reduce

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray, UInt64
import serial


class PubToSTM32(Node):
    def __init__(self, name):
        super().__init__(name)
        self.declare_parameter("device", "/dev/ttyUSB0")
        device = self.get_parameter("device").get_parameter_value().string_value
        self.car_error = None
        self.local_coord = None

        self.serial = serial.Serial(device, baudrate=115200, timeout=0.1)
        self.sub_car_error = self.create_subscription(Float64MultiArray, "/car_err", self.get_car_error_callback, 1)
        self.sub_local_coordination = self.create_subscription(
            Float64MultiArray, "/local_info", self.get_local_coord_callback, 1
        )
        self.sub_running_status = self.create_subscription(
            UInt64, "/running_control", self.get_running_status_callback, 1
        )
        self.timer = self.create_timer(0.2, self.timer_callback)

    def get_car_error_callback(self, msg):
        self.car_error = msg.data

    def get_local_coord_callback(self, msg):
        self.local_coord = msg.data

    def get_running_status_callback(self, msg):
        status = msg.data
        if status == 1:  # start
            self.run = True
        elif status == 2:  # pasue
            self.run = False
        elif status == 3:  # resume
            self.run = True
        elif status == 4:  # stop
            self.run = True
        else:
            self.get_logger().error(f"get unrecoginized control code: {status}")

    def timer_callback(self):
        if self.car_error is None:  # or self.local_coord is None:
            self.get_logger().debug(f"now msg is not received")
        else:
            if self.run:
                lat_err = self.car_error[4]
                yaw_err = self.rad2deg(self.car_error[5])
                data = self.construct_msg(lat_err, yaw_err)
                # self.get_logger().info(f"{data}")
                self.serial.write(data)
            else:
                self.get_logger().info("running control is pause")

    @staticmethod
    def rad2deg(rad):
        return rad * 180 / math.pi

    @staticmethod
    def construct_msg(lat_err, yaw_err):
        def pack_data(num):
            s0 = 0 if num > 0 else 1
            n = math.fabs(num)
            l1 = int(n / 100) % 10
            h2 = int(n / 10) % 10
            l3 = int(n) % 10
            h4 = int(n * 10) % 10
            h5 = int(n * 100) % 10
            return [s0 << 4 | l1, h2 << 4 | l3, h4 << 4 | h5]

        lat_err = 999.99 if lat_err > 999.99 else -999.99 if lat_err < -999.99 else lat_err
        yaw_err = 999.99 if yaw_err > 999.99 else -999.99 if yaw_err < -999.99 else yaw_err

        data = [2] + pack_data(lat_err) + pack_data(yaw_err)
        xor_chk = reduce(lambda x, y: x ^ y, data, 0)
        data = [1] + [42] * 7 + data + [xor_chk] + [42] * 8
        assert len(data) == 24, "data length error, it should be 8."

        return bytes(data)


def main():
    rclpy.init()
    rclpy.spin(PubToSTM32("to_stm32"))
    rclpy.shutdown()


if __name__ == "__main__":
    main()
